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CALIBRATION FOR 3D MEASUREMENT SYSTEM 

This application claims priority from provisional application 60/421,916, filed October 
29, 2002. 

Field of The Invention 

The present invention relates to the field of non-contact laser scanner profilometer 
systems and calibration techniques for such systems. 

Background Of The Invention 

The present invention describes highly accurate techniques for calibrating 3D 
measurement systems. The theory and use of these inventions are introduced by examining how 
these inventions aid the construction and use of a non-contact laser scanning system. A body of 
useful prior art for this work is described in U.S. Patent No. 6,441,908, issued to Johnston et al. 

These and other advantages of the present invention will become more fully apparent 
from the detailed description of the invention hereinbelow. 



Summary of the Invention 



The present invention is directed to a method for providing calibrated geometric data 
from a three dimensional measurement system, the method comprising: providing a reference 
object having a predetermined geometric structure; providing reference object geometric 
structure data in a first coordinate frame, wherein the reference object geometric structure data 
corresponds to the predetermined geometric structure; providing a three dimensional 
measurement system comprising: a measurement element that measures geometric data in a 
second coordinate frame from the reference object and from a target object; and a motion system 
that provides relative motion between the reference object and the measurement element, 
wherein the relative motion occurs in a third coordinate frame, wherein the three dimensional 
measurement system provides motion data in the third coordinate frame corresponding to the 
relative motion; acquiring a geometric data set comprising the measured geometric data from the 
reference object by the measurement element as a result of the relative motion provided by the 
motion system, wherein the geometric data set further comprises the motion data; determining a 
first transformational relationship between the second coordinate frame and the third coordinate 
frame, wherein the first transformational relationship is determined using the measured 
geometric data and the motion data contained within the geometric data set, and using the 
reference object geometric structure data; and providing calibrated geometric data in a single 
coordinate frame from the target object by using the first transformational relationship. 

The present invention is also directed to a method for providing calibrated geometric data 
from a three dimensional measurement system, the method comprising: providing a reference 
object having a predetermined geometric structure; providing reference object geometric 
structure data in a first coordinate frame, wherein the reference object geometric structure data 
corresponds to the predetermined geometric structure; providing a three dimensional 
measurement system comprising: a measurement element that measures geometric data in a 
second coordinate frame from the reference object and from a target object; a first support 
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system that supports the measurement element, wherein the first support system resides within a 
third coordinate frame, wherein the first support system is capable of supporting the 
measurement element in at least a first element position and a second element position; and a 
second support system that supports the reference object, wherein the second support system 
resides within a fourth coordinate frame, wherein the second support system is capable of 
supporting the reference object in at least a first object position and a second object position; 
wherein a first relative motion occurs between the measurement element and the reference 
object, wherein the three dimensional measurement system provides first motion data 
corresponding to the first relative motion, wherein the first motion data includes either or both 
first position data in the third coordinate frame from the first support system and first position 
data in the fourth coordinate frame from the second support system; and wherein a second 
relative motion occurs between the measurement element and the reference object, wherein the 
three dimensional measurement system provides second motion data corresponding to the second 
relative motion, wherein the second motion data includes either or both second position data in 
the third coordinate frame from the first support system and second position data in the fourth , 
coordinate frame from the second support system; acquiring a first geometric data set comprising 
first measured geometric data from the reference object by the measurement element as a result 
of the first relative motion, wherein the first geometric data set further comprises the first motion 
data; acquiring a second geometric data set comprising second measured geometric data from 
the reference object by the measurement element as a result of the second relative motion, 
wherein the second geometric data set further comprises the second motion data; determining 
both a first transformational relationship between the second coordinate frame and the third 
coordinate frame, and a second transformational relationship between the first coordinate frame 
and the third coordinate frame, wherein both the first transformational relationship and the 
second transformational relationship are determined using the first measured geometric data and 
the first motion data contained within the first geometric data set, and using the reference object 
geometric structure data; determining both a third transformational relationship between the 

-3- 



second coordinate frame and the third coordinate frame, and a fourth transformational 
relationship between the first coordinate frame and the third coordinate frame, wherein both the 
third transformational relationship and the fourth transformational relationship are determined 
using the second measured geometric data and the second motion data contained within the 
second geometric data set, and using the reference object geometric structure data; determining a 
fifth transformational relationship between the third coordinate frame and the fourth coordinate 
frame, wherein the fifth transformational relationship is determined using the second 
transformational relationship and the fourth transformational relationship; and providing 
calibrated geometric data in a single coordinate frame from the target object by using the fifth 
transformational relationship and using either or both of the first transformational relationship 
and the third transformational relationship. 



Brief Description of the Drawings 

In order that the manner in which the above-recited and other advantages and objects of 
the invention are obtained can be appreciated, a more particular description of the invention 
briefly described above will be rendered by reference to a specific embodiment thereof which is 
illustrated in the appended drawings. Understanding that these drawings depict only a typical 
embodiment of the invention and are not therefore to be considered limiting of its scope, the 
invention and the presently understood best mode thereof will be described and explained with 
additional specificity and detail through the use of the accompanying drawings. 

Figure 1 is an isometric schematic view of a laser scanning system including a Rotary 
Stage and X Stage mounted on a rigid Base and Riser structure. The Scanner is mounted on an 
Arm attached to the moving portion of the X Stage. The triple line denotes the Rotary coordinate 
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frame, double line denotes the X coordinate frame and single line denotes the Scanner coordinate 
frame. 

Figures 2a-2e illustrate several views of an preferred precision monument design 
nicknamed "Mayan". 

Figures 3a-3e illustrate several views of another preferred precision monument design 
utilizing a 6 point pyramid monument configuration. The monument has built-in support legs 
that protect the monument surface and support it while standing on end. Ends of the monument 
contain shallow relief step gauges for verifying scanner performance. 



Detailed Description of the Preferred Embodiments 

Reference will now be made to the drawings wherein like structures are provided with 
like reference designations. It will be understood that the drawings included herewith only 
provide diagrammatic representations of the presently preferred structures of the present 
invention and that structures falling within the scope of the present invention may include 
structures different than those shown in the drawings. 

The inventions in this disclosure are intended for, but not limited to, use in a non-contact 
laser scanner profilometer, similar to the systems discussed in U.S. Patent No. 6,441,908. 
Examples of the implementations and identification of the preferred techniques will be made as 
they relate to laser scanning profilometry applications. 

This disclosure describes an automatic technique for simultaneously determining the 
rigid-body coordinate transformation that will align the coordinate frame of a substantially 2D 
laser scanner with the coordinate frames of several different motion axes. 



Figure 1 shows a 3D laser scanner configuration supported by a preferred embodiment of 
the present invention. The system shown comprises a Laser Scanner mounted on the X Stage, a 
linear motion device. The X Stage is rigidly affixed to the Riser, which is in turn connected to 
the stable Base. The Base also supports the Rotary Stage that supports sample parts in the field 
of view of the Scanner. The coordinate frames of the Scanner, X Stage and Rotary Stage are also 
depicted. 

The 2D laser scanner measures surface locations along a substantially planar cross 
section of a sample part placed on the Rotary Stage. The surface location data can be considered 
to be in the coordinate frame of the scanner. That is, each X, Y, Z surface location is specified 
by a set of axes and an origin location physically tied to the location of the scanner. The 
coordinate frame of the scanner will be identified as the SCAN FRAME and denoted with O s . 
Referring to Figure 1 , the planar field of view of the Scanner is substantially parallel to the YZ 
plane in Os. 

To acquire 3D data from the surface of the part, relative motion must occur between the 
scanner and the part. As shown in Figure 1, a straightforward method of accomplishing this is to 
attach the Scanner to a linear motion stage. It is convenient to describe the motion of this X 
stage in its own coordinate frame, the X FRAME, ideally aligned with the motion of the stage. 
The X FRAME will be denoted as <P X . N 

To construct a 3D data set describing the surface locations along a part using the system 
in Figure 1, the axial position information from the X stage must be accurately combined with 
the Scanner data to yield 3D data. However, as can be observed in Figure 1, <E>s is not aligned 
with O x . Therefore, directly combining the X position data to offset the Scanner data would 
result in a warped data set that does not accurately describe the surface of the part. The data set 
deformation would appear identical to a 3D shear transformation applied to the data set. 
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To correct the warped data, each data point in O s needs to be transformed into O x before 
it is combined with the X position data. A calibration method is required to determine the 
coordinate transformation, denoted v J / s2x that accurately maps data from Os to O x . The use of 
this transformation would allow the Scanner data to be combined with the X stage data, resulting 
in an accurate 3D data set in O x that describes the surface of the part scanned. 

A part rigidly affixed to the surface of the Rotary stage would occupy the location in 
Figure 1 occupied by the axes showing the coordinate frame of the Rotary stage (9 FRAME). 
The 0 FRAME will be denoted by O 0 . If the part is opaque, only one "side" or section of the 
part's surface can be digitized by the scanner. However, by rotating the position of the rotary 
stage, another section of the part's surface can be digitized. It is convenient to describe Oe such 
that the rotational repositioning that occurred between scans was a pure rotary motion. 

Since Oe is probably not aligned with the O x , a method of calibration is needed to 
determine the coordinate frame transformation, denoted that allows the digitized surface 
data set in O x to be accurately projected into Oe. However, multiple data sets projected from O x 
would land on each other in O 0 unless the rotary motion of the stage is accounted for. If the 
position of the rotary stage was recorded with each different data set, then a pure rotary 
transformation, 4^ can be applied to each data set. The resulting 3D data sets in O 0 can be 
appended together to accurately describe the surface of the part, even though the data was 
acquired with the part in different poses. Overlap of the individual data sets (or point clouds) is 
expected and acceptable, as long as there is no residual offset between the data sets and all the 
points are accurately projected into a single coordinate frame. 

Note that the part can have its own coordinate frame not aligned with Oe. For instance, a 
CAD model describing the geometry of the part would likely be referenced to some fiducial 
features on the part, such as three planes describing a corner. If the part is mounted in a tipped 
state on the Rotary stage, the part frame will not align with the Oe but will be related by a 
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coordinate frame transformation. The coordinate frame of a part will be referred to as <D PA rt. In 
the case where the part is a reference monument, then the coordinate frame of the part will be 
referred to as Om- 



Coordinate transformation mathematics in general is widely discussed in previous 
literature, a lesson in such is not included here. However, a few key concepts are important to 
review in detail. Each point we discuss can be represented by a vector with a [X, Y, Z] value as 
shown in Equation 1. This disclosure is mostly interested in Rigid Body coordinate 
transformations, defined as those spatial manipulations a rigid body could experience. They 
have two components, a pure rotation part and a pure translation part. Rotational transformations 
can be expressed in the form of 3x3 matrices. When a data point in one frame is multiplied by a 
rotation matrix, the location of the point specified in a new frame rotated around the origin of the 
original frame is achieved. Translation transformations can be expressed in the form of 3x1 
vectors. When a translation vector is added to a data point in one frame, the location of the point 
specified in a new frame shifted from the original frame is achieved. The use of both rotation 
and translations can be expressed in the same equation, as shown in Equation 1 where R is the 
rotation matrix and T is the translation vector. 
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Equation 1. 



Pure rotation matrices are always orthonormal, meaning they have the special property 
that their inverse is identical to their transpose. Other types of point manipulation can be 
expressed in matrix form, including scaling, reflection and perspective operations. These are 
operations a rigid body could not experience and their matrix representations are not 
orthonormal. Rotation and translation operations can be expressed simultaneously in a single 
4x4 matrix shown in Equation 2, know as a homogeneous coordinate system. 



Equation 2. 
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Note that a data point is now represented as a 4x1 vector. The homogeneous form in 
Equation 2 is mathematically equivalent to the rotation followed by a translation shown Equation 
1 . A homogeneous matrix that represents a rigid body transformation will have zeros in the 
locations specified in Equation 2 and the 3x3 sub-matrix identified by elements a-i will be 
orthonormal. Both rotation + translation as well as homogeneous representations are used in this 
disclosure. Even though the homogeneous representation is more compact and easier to 
manipulate, the calibration and alignment algorithms require the rotation and translation be 
handled separately. Note that the H transformation in Equation 2 is the form of the *F 
transformations we are seeking. 



Desired Calibration Technique Specifications : The desired specifications for a 
preferred embodiment of the 3D calibration technique of the present invention (which are 
deceptively simple while the implementation of them is not) are listed as follow: 

• The technique should preferably be able to determine the transformation V } / S 2x to 
project O s to O x . 

• The technique should preferably be able to determine the transformation v F X 2e to 
project Ox to Oe. 

• The technique should preferably produce only rigid body transformation results, 
without perspective, reflection, or non-symmetric axis scaling. 

• The technique should not rely on precision alignment of the components and their 
relative coordinate frames. Although precisely aligned components would justify 
assumption that the rotation and/or translation transformations are null, such alignments 



will not stand over time and are not easy to support once the instrument is deployed in the 
field. 

• The technique should not rely on precision external measurements, such as 
calipers, to obtain transformation values. These can be used to attempt to verify the 
accuracy of a calibration but are not reliable enough and are not easily automated. 

• The technique should preferably be a process that can be automated, e.g. can 
determine the various required transformations without the operator doing more that 
setting up and invoking the process. 

• The technique should preferably be able to produce a calibrated 
transformation with enough accuracy so that a Laser Scanner with an approximately 6" 
stand off and an approximately 12" deep x 60° wide field of view, used in a configuration 
similar to that in Figure 1, will generate less than about 0.001" of point to surface error 
due to deformations of the data set. 

• The technique should preferably be able to produce a calibrated ^xie 
transformation with enough accuracy so that a Laser Scanner with an approximately 6" 
stand off and an approximately 12" deep x 60° wide field of view, used as in Figure 1 to 
acquire part data from multiple viewing angles, will generate less than about 0.001" point 
to surface error due to misalignment of the coordinate frames. 

An important technique desired by the present invention is a method for aligning a pair of 
three-dimensional data sets. Different algorithms have been useful for aligning different kinds of 
data. Here we are most interested in a technique known as the Iterative Closest Point (ICP) 
algorithm. Of specific interest is how the ICP algorithm accomplishes the task of finding the 
rigid body transformation that will align two sets of three-dimensional data once a point-by-point 
correspondence has been established. A related task of interest is how a similar alignment can be 
accomplished starting with a single set of three-dimensional data and a three-dimensional 
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description of a surface. Some of the key concepts useful in understanding how ICP functions 
are: 

Correspondence: ICP works on two point sets of identical size. There must be a point-by- 
point correspondence between the two data sets. If a data set of Appoints was transformed using 
Equation 1 or Equation 2, a new point set with perfect correspondence to the original set would 
be generated. Each point in the new set has a corresponding point in the original set and a single 
rigid body transformation will bring the two data sets into perfect alignment, with the sum of all 
residual distances equal to zero. 

Mean Centering: ICP determines the best rigid body transformation to minimize the RMS 
error between two corresponding data sets. The first step in this process is to center each data set 
so its centroid is located at the origin of the coordinate frame. This is accomplished by 
calculating the centroid of each data and subtracting it from each point in that data set. The 
result is a data set that has been "centered" to around the "mean" location in that set. The values 
used for this centering have another important use described later. 

SVD Alignment: Once a set of corresponding data sets have been mean centered, the 
rotational transformation that yields the minimum RMS residual error between the data sets can 
be calculated. Of particular interest is how the Singular Value Decomposition (SVD) algorithm 
is utilized to determine this rotation transformation. First, a 3 x 3 covariance matrix is computed 
by multiplying the first data set by the transpose of the second. The SVD is computed on the 
covariance matrix, resulting in two orthonormal matrices related to the eigenvectors of the 
original matrix and a diagonal matrix with values related to the eigenvalues of the original 
matrix. A rotation matrix is then computed from the orthonormal matrices. Using the two 
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centers determined during mean centering and the rotation matrix, a rigid body transformation in 
the form of Equation 1 can be derived and used to align the two corresponding data sets. 

Iteration: The preceding explanation describes aligning two corresponding data sets and 
does not require any of the iterating that gives ICP part of its name. ICP techniques are typically 
run on dense data sets where a subset of one full set is chosen to drive the alignment. The 
challenge then becomes selecting the correct corresponding subset of points from the second set. 
The ICP algorithm addresses this challenge by allowing the selection of an imperfect subset of 
corresponding points. The alignment is calculated and applied to one of the data sets, resulting 
in a closer alignment that is not usually perfect. The process of selecting a new corresponding 
subset of points from the second set is repeated, along with the alignment and transformation 
steps. In this fashion, the alignment between the two data sets is refined, until a termination 
criterion is reached. 

There are countless published variations of strategies in how the subsets of corresponding 
data are chosen. One in particular, so called "normal shooting" relates to a preferred 
embodiment of the present invention. In this strategy, a plane is fit to the a small group of points 
in the second data set that are determined to be near a sample point in the first data set. If the 
plane fits the group well enough to accurately describe their location relative to each other, then 
a normal to that plane is "shot" through the sample point from the first data set. The point where 
this normal intersects the plane is determined to be the corresponding point in the second data set 
to match the sample point in the first data set. If the plane didn't describe the small group of 
points, another sample point is chosen and the process repeats until the desired number of 
corresponding points have been obtained from both data sets to conduct the alignment step. This 
same approach would also work in situations where, instead of two point clouds, one point cloud 
and a description of a surface are to be aligned. In that case, the process for choosing 
corresponding points would be similar but instead of shooting normals from planes fit to points, 

-12- 



the normals would be shot from the various sub-surfaces and the shortest normal would be used 
to generate the point corresponding to the sample point. 

Features of the 3D Calibration : The capabilities of known ICP algorithms were 
intriguing but did not provide solutions to match the desired specifications for a preferred 
embodiment of the 3D calibration technique of the present invention. As part of the calibration 
technique, we were forced to invent a new nested iterative alignment algorithm capable of 
aligning two coordinate frames simultaneously. This is one of the many unique contributions of 
this calibration technique. Other unique features include, but are not limited to: 

• We decided to utilize the concept of aligning a point cloud to the description of a 
surface. 

• We decided to scan an artifact with well known and substantially accurate surface 
description, denoted as "the monument", to generate the point set used in the alignment 
process. 

• We wanted a monument that could be substantially accurately described entirely 
by substantially triangular facets. These are easy to construct, easy to measure to verify 
geometry and easy to describe mathematically. 

• After studying a preferred monument shape, we generated a unique class of 
monument forms that provide superior alignment performance with a preferred 
embodiment of the 3D calibration technique. These monuments also contain unique 
features that make them preferable for verifying the performance of the complete scanner 
system. 

• By placing the monument almost anywhere in the field of view of the scanner, an 
automatic routine can be invoked to scan the monument and generate a highly accurate 
4 / s2x transformation. 
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• By placing the monument on the rotary stage and scanning at two (or optionally 
more as desired) rotary positions, an automatic routine allows both the 4 / S 2x 
transformation and the ¥x2e transformation to be substantially simultaneously 
determined. 

• The technique also allows the pose of the monument or other similar objects to be 
automatically determined in O x or G> 0 . 

Disclosure Format : The algorithm(s) for conducting a preferred embodiment of the 3D 
calibration technique of the present invention are disclosed in detail in Tables 1 and 2. These 
algorithms have been prototyped in the interpretive mathematics environment Mathcad (Mathcad 
2001 Professional, MathSoft Inc. 101 Main St. Cambridge, MA 02142). Mathcad documents 
have the significant advantage that the mathematics is presented in a notionally correct graphical 
format that is easy to read and readily understandable. Since no complier is involved, a novice 
could recreate the entire algorithm by transcribing them directly into Mathcad. As such, these 
documents are essentially self-documenting in a fashion that could never be obtained with C 
code. In addition to benign formatting, Tables 1 and 2 have also been heavily commented to 
further aide in understanding the algorithm. 

The only slight drawback to this form of algorithm disclosure is that the Mathcad sheets 
were specifically developed to aid in the development and testing of the algorithm. As such, 
they contain places where the diagnostics and function definitions seem to serve little purpose. 
Therefore, the following description of the algorithms will serve as a guide to the appropriate 
sections of the algorithm and deal with the more relevant concepts developed and implemented, 
avoiding the mathematical details wherever possible. 

XICP Algorithm : Table 1 contains the algorithm denoted in entirety as "XICP". This 
algorithm requires as input a data set of points measured from the surface of the monument, an 
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accurate surface description of the monument and a calibration file from the scanner. The output 
is a 4 / s2x transformation matrix and a v Fx2M transformation matrix, as well as a host of 
diagnostics. The details follow. 

Points: The points are given as a hybrid tuple of [xx, ys, *s] where the x value comes from the 
position of the X stage when they and z values were acquired from the scanners measurement of 
the monument surface. Note that x is in 3> x while (y, z) is in O s . The actual data shown 
throughout Table 1 is simulated data that was generated from the surface definition of the 
monument. As such, it contains no measurement noise. Therefore, its final alignment with the 
monument surface is limited only by numerical precision and any termination criteria. Since the 
data is simulated, the exact v Fs2x and 4 / x2m transformations used to create the data set are known, 
so the efficacy of the algorithm at recovering these values can be investigated. The data is read 
into the algorithm in the section OH. 

Monument: The surface of the monument is described by a list of triangular facets. Each facet 
is described by its three corner points. The monument definition file has two kinds of entries. 
The first is an indexed list of points that form vertices of triangles. The second is an indexed list 
of facets and which points contribute to each facet. The monument file is read into the algorithm 
in section 1 A. Since the monument surface is specified only by a list of points in a specific 
order, the monument definition can also be transformed by rigid body transformations, such as 
Mean Centering, without affecting the accuracy of the surface description. The triangular facets 
are converted to a parametric representation in section IE and are dealt with in that fashion 
throughout the algorithm. 

Calibration File: The calibration file is read into the algorithm in section 01. Examination will 
show the value have all been set to 0 because the so called "Planar Comp" was not used when 
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generating the simulated data. However, in a real scanner used in a preferred embodiment of this 
invention, a "planar compensation surface" is a requirement to maintain high precision. The 
scanner sweeps the laser beam across the field of view preferably utilizing a rotating polygon 
mirror. The sweep is typically close to but not precisely described by a plane. A calibration 
polynomial is used to describe the shape of the path swept out by the laser. The resulting 
corrected point location in O s is a tuple given by [Ax(y, z), y 9 z\ Since Ax is a function of (y, z) 
it need not be passed into the algorithm along with the [x x > ys, z s ] tuple as it can be calculated 
when needed using the polynomial coefficients. Accommodating this correction factor 
complicates the entire algorithm simulation process and although it is often left out in the 
simulated data, it is dealt with rigorously in the XICP algorithm. 

Simulated Data: In the simulated example in Table 1, the scanner has recorded surface data 
locations from the facets of the precision monument. The Os is transformed away from Ox by - 
1.18532° around the Z-axis and 4.03747° around the 7-axis. Therefore, the data from the 
monument will appear sheared. Additionally, the pose of the monument is not orthogonal to Ox, 
but is off by -5.00847° around Z, 0.92235° around Y and 4.81052° around X. The function that 
does all the iterative manipulation of the data to compare it to the monument surface can be 
found in section XX. For whimsical reasons, this algorithm is called "Ballet" because it 
repeatedly moves the data back and forth between the coordinate frames. 

Rough Alignment: The first two lines of Ballet load the data set into temporary buffers. Several 
sets of data are created from the original data set, understanding their purpose is import in 
understanding the functioning of the algorithm. There is the initial hybrid data set "Data". Then 
there is "SKEW_Data" which is a copy of the original data points that are in O x and are 
iteratively corrected by the progressively better values of Ts2x. There is also "TEMP_Data" 
which are the iterative corrected values of "SKEW_Data" that are projected into the frame of the 
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monument, O m , using progressively refined values for v Fx2m- Of course, initially we have no 
idea of l Fs2x or X i / X2M so all three sets start identical. It is very important to note that each time a 
more refined value of ^x or v F X 2m is determined they are applied to the original "Data" to get 
"SKEW_Data" and "TEMP_Data". If we modified the values of "SKEW_Data" and 
"TEMP_Data" by incrementally adjusting them with each pass of the algorithm, we would 
obtain refined and accurate values of points on the monument surface in both O x and O m , but 
would have lost track of how we got there. By always creating "SKEW_Data" and 
"TEMP_Data" directly from the original "Data" we ensure we obtain the desired values of ^^x 
and 4 / x2M. Note that "Data", "SKEW_Data" and "TEMP JData" are kept the same length so that 
all corresponding copies of a point have the same index value. 

The 3 rd line mean centers the data if desired, refer to section OB. It is usually 
recommended to mean center the data and the monument as a first step, in this example the 
monument definition was already centered. The XICP algorithm as disclosed here is intended as 
a final alignment technique for data sets that start somewhat aligned with the monument, for 
example +/- 30° and +/- 15". Greater initial misalignments must be handled with an initial 
annealing step, known in the art but not discussed here. 

The 4 th line initializes a series of variables to 0. The 5 th line sets a flag that will instruct the 
algorithm to conduct an initial alignment before trying to simultaneously determine *Fs2x and 
v Px2M- The 7 th line graphically defines an outer loop that will run 101 time or until halted. This 
detailed simultaneous determination loop does not start until a rough alignment is completed. 
Evaluation falls to the 23 rd line where the inner, rough alignment loop runs an arbitrary number 
of times, in this case 21 times or until sufficient alignment convergence is obtained. 

The inner loop is implementing an iterative loop similar in some aspects the normal shoot 
to a surface described previously. An important aspect of the inner loop is which data set it is 
working on and how it shares them with the outer loop. 
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Each m ih point of the "TEMPJData" points in G> M are shot against each facet in the 
monument in the 29 th line and either the closest facet or a list of identically close facets is 
returned. The facet or list of facets is evaluated to find if the projected point on the surface of the 
closest facet is inside the boundaries of the facet. If the projected point is inside the boundaries 
of the facet, it is now accepted as a control point with correspondence to the m ih original data 
point. Copies of the m th data point from "Data" and "SKEW_Data" as well as the new m ih 
control point are put into temporary buffers denoted as "SUB_Data", "SUBSKEWData" and 
"SUB_Control" respectively. 

When all the valid points and corresponding control points have been accumulated, mean 
centering and SVD alignment are conducted in lines 40, 41, and 42 to align the 
"SUB_SKEW_Data" with the "SUB_Control" data. In this fashion, a progressively refined 
value of v Px2M is determined using a progressively refined copy of the original data in Ox. In the 
initial 21 rough alignment passes, there is no difference between "Data" and "SKEW_Data", so 
the alignment accomplishment are limited to moving the sheared original hybrid data closer to 
the monument, the shear effects cannot yet be addressed. The new values of x P X 2m just 
determined is applied to both "SKEW_Data" to generate a refined copy of the "TEMP_Data" set 
that is closer to the monument. 

In line 44, the new values of l F X 2M are also applied to "SUB_SKEW_Data" to move that 
data subset closer to the monument. This new "TEMP_SUB_Data" is then compared with 
"SUB_Control" data and the RMS distance between the two sets is calculated as a performance 
metric. The inner loop will run 21 times or it will break if the RMS values of 3 successive 
passes within a tolerance value of each other. In the example in Table 1, the tolerance is set to 
0.0000001. For reference, the top portion of the "RMS Dist. Error V. Iteration Pass" plot shows 
the progress of the rough alignment phase. The RMS went from about 0.10" inches to 0.01" 
inches in 20 steps. It appears the rough pass alignment terminated because it had bottomed out. 
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Simultaneous Alignment: Once the successive rough alignment passes terminates, the 
whimsically named "PingPong" flag is set and from now on the algorithm will take a single step 
of refining v F S 2x (outer loop) followed by a single step of v Px2M (inner loop) until 101 passes 
have been taken or the algorithm bottoms out and terminates. Starting again at line 9, a series of 
data manipulation steps are conducted on the corresponding "SUB_Data" and "SUB_Control" 
data sets determined on the last pass through the inner X2M loop. First, the inverse of the most 
refined values of v Px2m are applied to the "SUB_Control" data set to project it into Ox using the 
whimsically named "Tango" function, reference section 6F. Now that both the control and 
original data sets are in Ox, the values for v Fs2x can start to be refined. 

A fundamental observation that makes the refinement of v Ps2x possible is that the shear in 
the hybrid monument scan data can be corrected by a rigid body transformation if the (ys, z$) 
points are transformed by x ¥s2x before the data is translated by the x\ term. After the 3D hybrid 
data is formed, a rigid body transformation cannot repair the sheared data set. The technique 
developed to take advantage of this discovery is to remove (subtract) the point-by-point xx term 
present in the "SUB_Data" set from both the "SUBJData" and "SUB_Control" data sets. When 
planar compensation data is not present in the data set (as when the data is simulated) then this 
"collapsing" step will yield a perfectly planar "SUB_Data" set denoted "COMP_X_SUB_Data". 
The corresponding "COMP_X_SUB_Control" data set will have residual X values that are due 
to the v Fs2x terms. The now familiar mean centering and SVD alignment step can be conducted 
on these two compressed data sets to generate a refined set of values for the v F S 2x transformation. 

The data collapsing steps take place on lines 1 1 and 12. Referring to the two enabled 
Compress Da ta and Compresscontroi in section 6C, we can see the functions allow for the presence 
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of a planar compensation surface in the form of an inclined plane. After the point-by-point x x 
term has been removed from both data sets, the Axs term is carefully added back in. 

After the refined values of v Fs2x have been determined, we can see that line 19 null out 
the rotation component about the X- axis. This term does not contribute to the shear of the 
hybrid data set in the preferred embodiment. Further, the rotation about the X-axis terms were 
observed to compete with the v J / x2M transformation values. Therefore, it is suppressed on each 
pass through the outer loop. The new, refined values for v F S 2x are now applied to the original 
data set "Data" to create the progressively refined "SKEW_Data" set in O x . The "SKEWJData" 
set is created on line 20 using a function in section 6B that very carefully removes the xx values 
from the original hybrid data set, leaving any Ax s terms. The ^^x transformation is then 
applied and the x x data is added back to the data. The "SKEWJData" is then moved back into 
cp M to generate the progressively refined "TEMP_Data" set, which is used in the next pass 
through the inner X2M loop. 

The double alignment loop works to align the data to the monument in O m , then moves 
all the data back into O x , collapses the data and works to align to the collapsed monument. By 
always applying the refined 4 / s2 X and V F X 2M to the original data set and iterating to refine each 
transformation in turn, the XICP algorithm can simultaneously achieve optimized values of both 
from a single data set in an automatic fashion. The dark lower line in the "RMS Dist. Error V. 
Iteration Pass" plot shows the XICP algorithm was able to refine the RMS distance errors 
between the simulated data set and the monument surface to better than one part in a millionth of 
an inch. All the angular components in V P X 2M were recovered to better than 0.00003°. The 
angular components in Ts2x were also recovered to within 0.00003°. 

There are several strategies employed to optimize the speed and efficacy of the XICP 
routine. One is shown in section 3H where points too close to the edge of triangle can be 
rejected and not included in the control data set. Another technique, not shown in Table 1, is 
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where a small percentage of the entire data points are utilized during the initial rough alignment 
and simultaneous optimization. After a certain tolerance level of RMS error has been obtained, 
the data can be resampled to optimize a larger data set. This speeds up the algorithm by reducing 
the number of comparison steps to be accomplished, yet still forces a highly accurate result. 

Finding ^ys q: The XICP algorithm in Table 1 provides refined values for v P S 2x and v F X 2m 
by matching a single scan of the monument to its surface definition. The x i f s2x transformation 
can be used to transform the Scanner data to align with the X-axis in Ox before it is combined 
with the position values from the X stage. In this fashion, calibrated data in O x is obtained from 
the Scanner + X stage combination. It is worth noting that the monument can be placed 
anywhere in the field of view of the scanner to achieve good results using XICP. This 3D 
calibration technique can be used on a system similar to that shown in Figure 1, with or without 
the rotary stage. However, if a rotary stage is to be used, it is very desirable to have refined 
values of v I / x2e so individual scans can be projected into Oe, enabling automatic, seamless 
knitting of data from multiple scans at different rotary positions. 

Using Two Poses: By rigidly mounting the monument on the rotary stage and scanning it in 
several different poses, the XICP algorithm will provide a V P X 2M for each pose. Since the 
monument was rigidly affixed to the rotary stage, these different values of V P X 2M are related by a 
pure rotary translation about the X-axis in O 0 . Table 2 contains a Mathcad sheet that specifies 
two different algorithms that may be used to find the value of ^fxze- The preferred, 
"RotCenterFromFrames" in section ID, utilizes the v Fx2M transformations from XICP analysis 
from two different poses of the monument rigidly affixed to the rotary stage. 

"RotCenterFromFrames" takes as arguments the inverses of the two v Fx2M transformation 
matrices, making them Tm2x transformations. It then defines two entirely arbitrary points (pi, 
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p2) in <D M . In the definition in Table 2, points 500 inches out on the Y and Z axes were defined. 
These points are then transformed back into O x using the two x F M 2x transformations values, 
resulting in two sets of two points (Pai, Pa 2 ), (Pbi, Pb 2 ) related by a pure rotation about the X 
axis in Oe. 

The bisector (Bsi) of the line connecting the two corresponding instances of the first 
point (Pai, Pbi) is computed, as is the bisector (BS2) of the corresponding line connecting the 
instances of the second point (Pa 2 , Pb 2 ). Next, the vector connecting (Pai, Pbi) is computed (Vi) 
and that for (Pa 2 , Pb 2 ), yielding (V 2 ). A third vector (V r ) is computed by taking the cross 
product of Vi and V 2 . The vector V r is parallel to the X axis of the rotary table. 

It remains to find a point corresponding to the origin of Oe that lies along the axis V r . 
Recall that the intersection of three non-parallel planes forms a point. One form of 
mathematically specifying a plane requires a normal vector and a planar offset value. One plane 
can be defined using Vi as the normal vector and the value for the planar offset can be found by 
taking the dot product of Vi with its bisector Bsi. A second plane can be found using V 2 and the 
dot product of V 2 and Bs 2 . Note that the intersection of these two planes is identical to the axial 
vector V r . The third plane required to find an origin of O 0 can be found using V r as the normal 
vector and picking an arbitrary point on the third plane to specify the planar offset value. In the 
example in Table 2, the point where V r crosses the YZ plane in O x was chosen. Given 3 
equations of planes with forms similar to ax + by +cz = d, a matrix operation can be conducted to 
yield the values of (x, y, z) that correspond to the intersection of the three planes and also specify 
the origin we are seeking. Therefore, the return from the function "RotCenterFromFrames" is an 
origin point and the axis of the rotary stage given in <Px. 

A function "AlignAxes" in section IE is used to specify the l F X 2e transformation. For 
input, it takes the output "RotCenterFromFrames" as well as the value of the origin in d>e (0,0,0) 
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as well as the specification of the axis to align to [1, 0, 0]. The result is the v F X 2e transformation 
that accurately maps the data from <D X to Oe. 

Using Three Poses: Another useful algorithm for finding v F X 2e is in section IB. 
"RotationCenter" requires v Pm2x from three different poses of the monument on the rotary table. 
A single arbitrary point (p) in <D M is projected into O x using the three 4 / M2x transformations, 
yielding three instances of the same point, (pi, p2, P3). These points are all related by a pure 
rotation about the X-axis in O e . To find the axis and origin, a similar strategy to the Two Pose 
case above is employed. Two vectors and their bisectors are defined. A third vector orthogonal 
to the first two vectors is defined using a cross product. This new vector is parallel to the X-axis 
of the rotary stage. 

To find the origin, three planes are defined. The first two are specified by taking the first 
two vectors and the dot product of the vectors with their bisectors. The third plane can be 
defined using the orthogonal vector and an arbitrary point. Preferably, the third plane can be 
chosen where the orthogonal vector crosses the Y, Z plane in <D X by setting the planar offset 
value to 0. The origin of the rotary frame can now be specified by the intersection of the three 
planes using the same reduction technique discussed in the Two Pose section. The return from 
"RotationCenter" is the origin point and the axis of the rotary stage given in O x . The function 
"AlignAxes" can then be used to specify the transformation that accurately maps the data 
from cD x to 3>e. 

Monument Shapes and Definitions : The preceding sections have referred to a preferred 
optimal monument design. Simulated data from a version of an optimal monument was included 
in Table 1. Here, we discuss the features and constraints of monument design that 
simultaneously make them useful in calibration 3D laser scanner system using the XICP 
algorithm and aid in the traceable verification of the scanner performance. A list of desirable 
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qualities in a precision monument used in conjunction with the XICP algorithm to calibrate a 
laser scanner system similar to that shown in Figure 1 includes, but is not limited to: 

• A preferred monument should preferably be constructed from a rigid, stable 
material such as a ceramic so that the precision of the features do not change over time 
and with environmental changes. 

• The precision reference surfaces should preferably all be described using 
substantially triangular facets. The monument may have non-precision surfaces in 
locations not intended for scanning, such as the back side of the monument or in the 
location of any mounting holes. 

• The non-precision surfaces will not affect the 3D calibration algorithms if they are 
not represented in the surface description of the monument, e.g. if a sparse surface 
representation is utilized. An example of a sparse surface representation can be found in 
Table 1, section 1G, where the pyramid features are depicted without any supporting 
structure around them. 

• The monument structure should preferably have features that exhibit significant 
slope changes. The idea here is that during alignment, a point moving off the intended 
facet should encounter an abrupt increase in distance penalty the farther it is misaligned 
from the ideal position. Therefore, gentle slope changes and small feature sizes should be 
avoided. 

• The monument structure should preferably have variations in all directions. For 
example, the pyramid monument in Table 1, section 1G has variations in the X, Y and Z 
directions. Long "ridges" instead of separate pyramids would make it hard for the XICP 
algorithm to correctly align because shifting along the long axis of the ridges would be 
hard to detect. 

• A preferred key feature of a successful monument is a structure that has 
approximately 25% of the reference surfaces sharing a common extended plane. The 
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reference surfaces are the precision surfaces that are represented in the surface definition 
file. These surfaces do not need to be contiguous. This amount of common plane area 
has the effect of forcing the XICP algorithm to keep the alignment of the point cloud 
engaged against the extended flat surfaces of the monument. Therefore, the rest of the 
alignment can be approximated as lateral motion along the extended plane and rotation 
around a normal to the extended plane. These constraints are similar to those obtained 
using a standard 1 2 3 alignment technique. Aligning against a monument such as the 
pyramid monument in Table 1, section 1G without any extended planar sections defined 
can let a slightly rotated alignment "hover" above the surface of the monument in order 
to minimize distance errors. Such alignments are not as accurate as those conducted on 
monuments with the extended plane. Empirical evidence suggests that the optimal range 
of the ratio of extended flat area to the total projected area of the reference surfaces is 
between about 15% and about 50%. 

• A monument should be large enough to span at least about 50% of the field of 
view of the scanner. A tiny monument in a large field of view will not have the leverage 
to ensure angular accuracy of the alignment sufficient to avoid measurement errors at the 
edge of the field of view. 

• For a monument to be useful as a tool used in both calibrating and certifying the 
performance of a scanner system, it preferably should have regular features that are easy 
to manufacture and to certify. Planar facets at angles of 90° and 45° are preferable from 
a manufacturing and verification standpoint. Modified pyramid structures such as those 
in Figures 2a-2e and Figures 3a-3e are preferable. 

• The use of structures related to pyramids allows the monument to contain integral 
step gauges. Examination of Figures 3a-3e shows that the monument was designed such 
that in all directions, the step distance of each face is exactly 2.000" from a neighbor 
pyramid face. This allows the monument to be scanned in a wide range of orientations in 
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the field of view and the geometry of the resulting scans can be easily checked using 
standard point to plane and plane to plane verification fitting routines. 

• A very useful feature of a monument is if it contains implicit points that can be 
verified through certification and found via scanning. An example of an implicit point is 
the center of a sphere. The center can be found via fitting a sphere to either scan data or 
verification CMM data. However, spheres are not a preferred monument feature when 
using laser scanners. We prefer to utilize implicit points defined by the intersection of 
three planes. In the truncated pyramid structures in Figures 2a-2e, each pyramid has four 
implicit points (i.e. on the corners of the square at the top of each pyramid) that can be 
both scanned and verified with a CMM. The monument can be scanned in many 
different poses in the field of view of the scanner. The accuracy of the scanner can be 
verified by analyzing the step features inherent in the planes and by deriving the relative 
position of all the implicit points and comparing against the traceable certification of the 
monument. 

Mayan Monument Example: With the exception of one dimension, the monument depicted in 
Figures 2a-2e is a preferred monument design for 3D calibration with the XICP algorithm and 
for use in scanner verification. It's advantages and features include, but are not limited to: 

• It is made, for example, of a rigid, stable, opaque ceramic. 

• It is large enough to span about 50% of the field of view of the laser scanner 
system in Figure 1. 

• The truncated pyramids have a preferred variation in all directions with large 
changes in surface normal at the interfaces between faces. 

• All the flat surfaces may preferably be described by triangular facets. 

• The manufacturing (and preferably the entire manufacturing) is preferably done at 
90° and 45°. 
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• The surface description of the monument preferably only includes the sides and 
tops of the pyramids. The large flat between the pyramids, mounting holes and small 
stand-off are not included. Therefore, the large flat need not be ground and lapped to 
precision, thereby reducing monument cost. 

• The shape is preferable for precision manufacturing and traceable verification. 

• The square flats represent substantially 25% of the total projected area of the 
reference faces. 

• The steps between the neighboring pyramid faces are, for example, substantially 
4.9497", an exemplary spacing of substantially 5.0000" would be preferable. 

• There are 4 implicit points on the corners of the 2" square at the top of each 
pyramid. 

Six Point Pyramid Monument Example: The monument depicted in Figures 3a-3e is also a 
very useful monument design for calibrating a 3D laser scanner using XICP and for verifying 
scanner performance. It's advantages and features include, but are not limited to: 

• The reference surfaces are the faces of the 6 pyramids and a portion of the flat 
section in the long valley between the rows of 3 pyramids. 

• The reference surfaces may preferably all be described by triangular facets. 

• All construction is preferably done at 90° and 45°. 

• The monument has enough variation in all directions to be preferable for use with 
XICP alignment. 

• The faces all have significantly different surface normals than their neighbors. 

• All the facets have a 2.000" step gauge relationship with neighboring pyramids. 

• The ends of the monument preferably have precision 0.002", 0.004" and 0.008" 
height gauge features for investigating the scanner resolution. 
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• The square post legs in the comers simultaneously provide support legs for 
standing the monument on any side face, they act as standoffs to protect the pyramids, 
and they also form 6.000" step gauges from face to face in one direction and 10.500" step 
gauges in the other direction. 

• One slight drawback with the specific implementation in Figures 3a-3e is the 
monument would preferably be larger than the Mayan configuration to better span the 
field of view of the scanner. 

Alternate Implementations : There may be various modifications and variations to the 
concepts and implementations disclosed here (including the algorithms) that are within the scope 
of the present invention including, but not limited to: 

• The XICP algorithm is not limited to characterizing and calibrating only 2-axis 
motion systems. If a multiple axis motion system (e.g. (x,y) or (x,y,z,a,b,c)) is pre- 
compensated, such as a CMM, then by aligning with one axis of motion, the scanner can 
be aligned with the whole motion system. 

• If the multiple axis motion system is not pre-compensated, then by aligning the 
scanner with one axis of motion at a time, the scanner, XICP algorithm and monument 
can be used to develop the compensation mapping to relate all the axes of motion into a 
common, compensated coordinate frame. 

• The motion system does not have to be configured like the system shown in 
Figure 1 . For example, the motion axis could be horizontal, or two crossed axes of 
motion could be used instead. Further, two orthogonal rotary stages could be used in 
place of the single rotary stage shown in Figure 1 . Finally, the scanner could be 
connected to a six axis CMM machine. 

• Multiple scan heads can be calibrated simultaneously using XICP and at least one 
monument. For example, if the system in Figure 1 had two scan heads, both 
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simultaneously measuring item(s) on the rotary stage, then using the monument(s) and 
XICP on the individual scan data, both scan heads could be automatically aligned with 
O 0 . Therefore, both scanners would be reporting data in the same coordinate frame, 
creating seamless data automatically with no need for post process alignment steps. 

• The monument shapes are not limited to planar facets or those described by 
triangular facets. Curves, spheres and arbitrary shaped surfaces could alternatively be 
used if other surface specifications were used, including but not limited to STL, IGES, 
STEP and SAT (well known surface description formats). 

• The monument can have lots of detail in addition to the triangular facets defined 
in the surface definition. Such detail can be useful in constructing or mounting the 
monument. 

• The monument can also have detail used for verification that is not used in the 
calibration step by simply not including the other feature's calibration surface description 
or by placing the details on a face of the monument not scanned during calibration. 

• The XICP algorithm can be sped up by performing a spatial sort on the facets 
including, but not limited to, a KD tree or its related partitioning systems. These 
partitions would allow far fewer facets to be tested with the normal shoot to find the 
control points, possibly providing significant speed increases. 

• Once a monument has been aligned to a scanner using XICP, its implicit points 
can be found. XICP can be used to find the relationship between two poses of the 
monument by aligning its implicit points. Table 2 can be modified to work on the 
implicit point data instead of the arbitrary points in order to find the center and axis of 
rotation of the rotary stage. 

• The monument is not limited to 4 sides. It may, for example, comprise 3 or more 
than 4 as desired. The faces of the features are not limited to 45° and 90° for a monument 
having any number of sides. 
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• The rotary table may be replaced by a linear table of the type, e.g., in Figure 1 . In 
such a case, Table 2 should be modified to find the new coordinate frame of the linear 
table. This configuration would fall within the scope of the present invention. 

• The scanner could be mounted directly on a rotary stage in order to change the 
orientation of the field of view between scans. The calibration would fall within the 
scope of the present invention. 

Those of ordinary skill in the art will recognize that various modifications and variations 
may be made to the embodiments described above without departing from the spirit and scope of 
the present invention. For example, the preceding techniques do not have to be implemented 
alone, they can be combined to produce hybrid techniques that might fit a specific application 
better than a single technique. It is therefore to be understood that the present invention is not 
limited to the particular embodiments disclosed above, but it is intended to cover such 
modifications and variations as defined by the following claims. 
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Table 1 

Table I contains the algorithm denoted in entirety as "XICP". This algorithm requires as 
input a data set of points measured from the surface of the monument, an accurate surface 
descripti on of the monument and a cal ibrati on file from the scanner. The output is a%2x 
transformation matrix and a ¥>au transformation matrix, as well as a host of diagnostics. 



XlCPCal F.mcd 



Future 3D Cal Simulation based on Future Cal Q.mcd 



OA: Define some 3D transforms 



Non-homogeneous individual rotations and translations 



Me)- 



r .cos(0) sin(e) oV 
ah(e) cos(e) o j 
o o: \) 



r cos(P) 0 -siri(p)} 

o i. o j ~ 

,sin(p) 0 cos(py 



1 0 0 \ 
0 cos(y) sin(y) | 
v 0 rrsiri(y) cos(y)/ 



y 



Nbn-homogeheous rotation matrix 

f vcos(e) • cos(p) sin (e)- cos (y) + cos (O)- sin(p)- sin (y) sin(e}- sin(y) - cos(e)- sin(p)- cos (y)~\ 
AR(8;.foy)> -sin(e)-cos(pJ cos(e) cosfy) - sm(e) sin(p) : sin(y) cos(e)sin(y) + sin(e) sin(p) cos(y) | 

cos(p).cos(y) / 



V 



sin(p) -cos (p)- sin (y) 

NOT USED: Homogeneous transformation matrix, 



ATCe^.Y^.y/z) := 



cos(e)-cos(p) sin(e)- cos(y) + cos(o) -sin(p)-sin(y) sin(e)-sin(y) - cos(e) : sin(p) cos(y) : x ^ 
-sin (e)- cos (P) cos (8) ■ cos (y) - sin( 6) siri(p)- sin (y) cos(e); sin(y) + sin (e); sin (p ) : cos (y) y I 
sin(p) -cos (p)- sin (y) cos(p)-cos(y) z I 



6 o 6 i ) 

OB: Afunction toflnd the cent ro id of each cloud, another to subtract It from each cloud 



Mf- Input 1 
for, k G O. .2 
N k <- mean(^) 



Fin d.Center wants only an array of points (in columns) and it will return a 
point that is the "center of mass" 



Center (Input, Center) := 



M<- Input 

for k€ 0... 2 



Center wants both an array of points (in cojumnsj and a single point at its 
center.. It will return a new array centered around the Center point. 
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PC: Now create the, Find the Rotation using the SVD on the Covarlance matrix 



Findg2 (Data /Control) : 



M«-Dat»CohtrolT 



Creat the covariance matrix: 



Find the left and right eigen vectors ; 
IJ^submatrixCN.O.^.O^) Split themout of Mathcadsbizzare return 
V sut^rnalnx^N, 3 . 5 ,0 ,2) Define the fp^ 
r<- v matrix; 



OP: We can also back out the actual rotation angles from the rotation matrix 

J' *t*l-.Q V\ 
,Sin ^cos(asin(lnpu^ : ^)^ 

in . » . ; — . ' u 

^cos(asui^ut2 Vo ))^ 

OE: Now we apply the rotation and fix the translation to move the Data to the Control 



It is possible to recover individual rotation terms.(angJes) by taking apart, 
the rotation matrix.. 



M6vc(Data,R, C^C coa ^h 



for ke 0.. cols(Data) - 1 

IDatatR <- R-Data 
^fii^W^ (^control " R * 
9 ata OfTset 



C data) 



Index variable to "active" columns of data 



U n- apply th e final transform to th e data 

This-deals with the pfrsets.in a column 
operation 



OF; Needed function to find distance from point to point 



Dp2p (Piiata , Pcontrol) := 



N*->M ^ 0 

for i € 0 .. cols(Pdata) - 1 

Ni <- | Pcontrol - Pdata^l 
rncan(N) 



This finds the distance from . 1st point to .2nd point. 



0G: Define a function to turn the Vertex Array into a Display Array 



Disp (Input) := 



M «- Input 



This is a bizarre function that takes a, matrix of column yectore ; and puts them into a form 
that for some.reasoh Math Cad will plot, but on ry if th ere are two data sets... Useless 
except for the compact notation it allows 
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OH: Needa file read/parse function 



Read(Name ; mirror ; Points) := 



Data <- RE ADPRN ( cohcat ( coricat( Name , " "))) 
j<-0 

for ie 0 . .. rows (Data) - \ , 
if. Data. , = mirror 

»,6 



This reads in the data file, strips off diagnostic data 
and. filters by mirror number, Can also skip points 
to provide "Points" total points in output set; 



M 



-Data 



VI. 



N <— M 



V Points. J 
(Out <h.N) .if Skip ." 0 
f for ke 6 ;: Points - 1^ otherwise 

[ Out*>^N* S ^i 
Out 



Test := Read(Data:Name, mirror i Points) rows(Test) = 3 f cols( Test) v= 136 

Test' := Disp(Test) 




Test',Test^ 

01: This function will drive the Planar Cbmp 

Xcoeff := READPRN("SirnXcoeff.irii'') 0 Readiin the Xcoeff matrix. Multiply by 0 to force to null^values.. 



Xcoeff=: 



fO 0 0 0 0 0 0 0"i 

0 0 0 0 0 0 0 0 

0 0 0 0 0 0 0 0 

0 0 0 0 0 0 0 0 

0 0 0 0 0 0 0 0 

{0 0 0 0 „0 0 0 0 ) 
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Xco e ffn^ror := submatrix (Xcoeff, mirror » mirror , 0 i cols(Xcoeff) 2) 0 Strips out last control coJumri 
Xcocff mirror = (° 0 0 0 0. 0 $) 



DcltaX(irjput) := 



for Je 0.. cols (input) — 1 



Given a set of XYZ data, return contribution of planar terms 



DcltaX 



1: Define a Monument 



iA: Read a MONUMENT file wltfrt Trlanlges defined 



Mon READFRN (concat(Mon_Name , "mon")) 



for ie 0^. rows(Mbn) -:l 



if Mon. rf,s 0 
1.3 



for me 0.. 2 
Vj.m x-Mohi*, 



if Mon 0 = 1 



for me 6.. 2 
Tnic, m Mon, m 

k :^:k,+ 1 



Scale : 



'i o 0} 
6 l o | 



Enablelthis tp read in a monument file, Go look at Scatter Points; 
X.mcd to learn how to read in -STL files 

ftiis takes apart the proprjetery monument deifinitibn file and prepares it 
into, a list of vertices and a list of the vertices involved in each facet. 



A useful Scale Matrix 



Scale :- 



1 0 of 
0-1 o I 

o o: -i) 
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V ;= ScaleV* The resulting Sea led. Vertex Matrix; 



MqhjDeh:= Find Cen te r ( v ) 
V :=- Center(V ,Mon_Cen) B 



Mon Ceh = 



lx 10 8 | 
1, 3.333x 10" 9 J' 



t his can bemused to center the monument, if its nc 
already there. 



I E: Find the u,v and /? vectors 



J := 0 .. rows(Tri) - 1, This is'the matser index variable- to reference individual triangles 



Q ■:= Tri <0) Vj := Tr£ l) V 2 := Tn 



V n := 



: <2>- 



< v i.> <Vo-X 

< v 2:> m 

Vj := V 1 - V 1 

UjX Vj 



Column vectors that -basically I ists which vertex is 
used in which:Corner by each triangle 

This is;the first edge vector of the triangles: 
This is, the second edge vector of the : triangles 

This is the normal Vector of the triangles 



1G: Define a list of vertecies in an order that allows MathCad to draw the "Mesh" 



V* ~ Disp(V) 



Thiscrea'tes the displayable; 
version of the vertex list 




1H: Define a Function to return a point on a Plane using parametric coordinates 

P 0 := READPRN(concat(Mon^Name/MORtxt B )) T SPECIAL CASE: Here, we are reading in P.O points ON the Monument, 

and Defining P:1 po.intsto.be IDENTICAL. The idea is that there is 100% 

Pj : = p 0 correspondance between the two data sets, XI CP has nothing to do and 

should reflect that. To test XI CP, we will then go about applying SKEW 
■p 0 > - Disp(P 0 ) For display... on, Y to ? A and see ' n 9 tf ^ can r ^er- 

Pj-^Disp^Pjj For display... 
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V-.Pi 



2: Perturb the Point Set off the Monument 



R M2X READPRN(cdricat(Mbh Nanre ,"Rin2x.txt rt )) This is cheating: for debug:.. . , . s • 

M2X ( 0.996 0.087 0.016 \ 

RM2X - -0 086 0,993 -6.084 | 

s -0.023 0.082 0.996 J. 

These functionsare now handles by an external point on iTionument simulator. See Scatter_Ppihts_Builder. 



3: Defined core XIGP Functions and run some Test cases 



3A: Use the function to find the centroid and then subtract it from each cloud 



Try it on the Control Points 

M 0 := Findcenter( p o) 

t 0 := c Center(P 0 ,M5) 
% := Disp(.T 0 ) 



Try it on the P..1 Points 
Mi: Fihd Ce nter(Pi) 
Tj := Center(P lt Mi) 
T.,. :~ Pisp(Tv) 



These should result'in data sets cenetered around 0,0,6 




























■Jfr* t* i «!9K 









0 


1 


o 


-0.208 


0.437 


1 


.2.184 


1.963 


2 


-0.103 


0.118 



V-..T, 



v\.T r 
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3B: Now create the Find the Rotation using the SVD on the Covariance matrix 

R' - FindR^fT^To); R' = 



f \ 0 0A 

o i o; | 
,6 o 1/ 



r 0:99605 0.08729 0.0161. \ 
-4.08565 0.99279 -0.08385 | 
v -0.0233: 0 : 08214 0.99635 J. 



3C: We can also back out the actual rotation angles from the rotation matrix 

f 4:915 \ 



Find Aiigles(RM2x): = 



-1335 |deg 
l,-4 : 713j 



0 |deg 



Find Ai^les(^ T ;) = 

3D: NoW we apply the rotation and fix the translation to move the Data to the Control 



Do all the motion 

P 3 := Move(P lf R';M 1 ,M 0 ) 
P 3 . ;= Disp(P3): 



Check just the rotations, 

P^-R'Pi, 
P.2». := Disp(P 2 ); 





vvp 3 , v,p 2 , 
3E: Needed function to find distance from point to point 



D P2P(P1^0)=° 
^(Ti^oJ-O 

PP2P( P 2> P 0) = ° 
D P2p( p 3^q),= 0 



Jnrtial.cbnditipn 

After centering,, before transformation 
Just rotation applied 

After full transformation 
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3H: Needed function to shoot normal and find distance from point to plane 

( < v 6 ..*)V This; s shpofe:thejppint..to 
£ -.(;poiftt:- v 



n (P6int;ihdex)r:= n-^-.t 



Ao) ■) 



mo 



> 121-10:-* 




>:121r10> 8 




>121 10 9 




>V121-10 9 


-0.397 




-0.397 




-0:397 




-0.397 


-0.692 




r0.692 




-0.692 




-0:692 


-1.089 




-1.089 




-1,089 




-1.089 


2 




2 




2 




2 


-2.397 




-21397 




-2.397 




-2.397 


1.308 




1;308 




1.308 




1.308 


-3.089 




-3.089 




-3.089 




-3089 



3G: Needed function to find x,y;z coordinate of normal shoot 

Pnonn (Point , index) :=: Point - <D nonn (Point , index) • ^ n<jcx This finds the XYZ location the normal shoot hit ipn^specjfic^facet 



Pl <°>= 



r M)208^ 
2.184 I 
v -0.103j 



^208 Y 
21"84< I 
^.103 ) 



2.121-10 8 



0:397 



/0:692 



1:089 



2.397 



1:308 



3.089 



'-oM 

v 0 178 


>:121-iO 8 




;0;397 


0.692 


1.089 


2 


.2.397 


1.308 


3.089 



3H: Needed function to find if x,y,z coordinate from normal shoot is out/on/ln triangle 

. , -5 

Aedge =' 1 X 10 
Test-,^ (Point t index) : 



Far = 0.5 



<fv 0 . ^> 



w <- Point 
VV ^ ("index "mdex) 
VV <~ (vmdcx-Vindex) 
("index' v mdex) 
WUi^ (^Uindex) 

UV-W-WrWU 

u 



This important function evaluates a: 
point on the plane defined by the 
corners of a than gluar facet and 
determines if the point is inside the 
facet or. outside. 



(yvr - uy.yv 
uv-wu- uu-wv 



if 



[(W) 2 ■■- uu-.vv] 

™ ■'<-[ S T0K ^[ T OK ^.(SOK ^ O)"]] 
SiN-^[(A«<ig« * s) (s < 1 - Aedge)] 
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TjN ^ ["(Acdgc I - AedgcJ] 

Sqk<- ( Ac< (s c s *4 ; (?^ F % T wt* + t ^f 1 - A«agc)] 

T^K^ (Acdgc ^ p (t^F^-S^fs + t.^ (l AcdgcJJ 
ON W^|hin<fc x ^P6i^ : ^ 

w:*- i (sqk + -Tok +: st qk)^° n 

IN 



K Ov. cols(P 0 ) - 1! 




1000 



31: F^or each point, findjthe index of the plane that is the minimum distance 



Indcx2 (Point) := 



for i € 0 rows(Tri) 

Mi , i ■ I>F2P (Point , P nonn (Point;0) 
NV;cs6it(M t l) 

for ie 0.. rows(Tri) - 1 

Out j .^Mj Q 

j i + l 
Out 



i2 k ::=\Indcx2(p 0 <^) 



What happened hereto generate' "I ndex2 B is: 

♦ some triangles have "sisters" on the same plane 

a point from one triangle will generate the same normal 
intercept for- all the sister triarigles:(same normals) 

♦ th e distance; from point to the plane will be identical for all 
sisters 

♦ only one sister triangle willactually Have the prpjected 
point inside of it, but sorting on; distance will-not 
guarantee;ch o.cjsin g;th e right triangle . 

♦ Often, the wrong trianlge was chooseh by the nearest 
sorting alcpritm, and a valid point was rejected because 
the projected point was notin the choosen triangle: 

So, an array of 1 or more index values comes out of "Iridex2'' 
and the XICP algorithm has been taughtto search each set of 
possible indices and find: one that actually has the projected 
point in it. This has the effect of allowing more points through . 

N EW: I have. added the "Safe comparison" feature so the 
points ori|y have to be within "Epsilon" of each other. We 
were experiencing floating point e^ 
files. ' 
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J k 0.5 " 



. k, 




0 200 400 600; 800 1000 

J-J= 340 

4: Define a XiGP basedi on the subf unctions 



0 200 409; 60* 800 1000 

k; 



Tol - 1 xlO 



- 7 This function performs standard formal Shoot" XIGP using an XYZ: dataiset : and a surface- consisting 

of triangu lar facets. 



X2M(Data) := 



TEMP/Data Data 

TEMPJData <r- Center(Data : Find Ca i tcr (Data)} if PRE.CEhOHR^FLAG 
for pass G O .iOO 

!Tndex;f6r the trimmed array" 

j<-6 

"Go through all tfae points in the data set" 
for m€:0\: co^ 1 

"Find indecies for the closest triangles, coul d be array of several si ster. triangles" 
ll m <r- Indcx2(TEK^Data( m >) 
"So, go ttirough the array and test each one" 
for ie 0.. rpws(l2 m ) - 1 

.™^S}<- Test^j" TEMP_P, (12 in). "| 

"Ah Ha - pick this one!" 
if TEMP_S = 1 

STJB_Data^^Data< m > 
SUB^Contro^ <- TEMP_P 

Cdate FindQcnt^CSTJB^Data) 
C C ontrol^^fccntcr( s ^-C?ntrol) 

Ri p *- Findp2 (Ccnter(STJB JData ,'G^) , Center (SUB^Control , C c?n ^ 0 j)) 
t^_Data >~%yc(Data,i^ 

lEMP^SUB^Data ^ Move (SUB_Data , ^ n „C^ U > c ^ol) 
Dp2p(SUB^Control , TEMP_SUB_Data) 



pass 



C6uritj n 



break 



if 



f RMS: r 



-RMS- 



'pass-1 



<Tol) if 



pass > 10 
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' TEMPData:^ 
County 



Call IGP 



Data tcnip > l 
Status I 
Count I 



This is it. Just define the the data set you want it to chew on and it will grind away to match it 
to the monumnet defined above. Returns are: 
:= X2M(Test) Data.temp = the last set of projections onto the monument. 

In a perfect world, these ARE the P.O set. 
Status -a vector of the RMS distance error for all the points, calculated at the end of each 
pass 

Gourit = the number of points that were used in each pass 
R = the final rotation matrix 



{ 0S955 -4 09149 -0.02473^ 
0.09323" 0.99225 0.0821 | 
^0.01703 -0:08404 0.99632 f 

^-5i35022\ 



0i9756 [dig: 
y 4X2129 j. 



Find Anglcs(R) ' 
^row^StatuO-a^ 0 ' 014 

pass := 0;i rows (Count) - 1 



K M2X 



■059605 -4.08565 -0.0233 \ 
0.08729 0.99279 01)0^^1% 
, 0.0161 -0.08385 0.99635 ) 



oi 

0 



Fuid AngIes( R M2X T ) = 



-5.00847^ 
0.92235 | dcg 
v , 4.81052 J 



D P2P(* > 0> P 1) = ° 
Dp 2p (P 0> D 3 ta tei] ^)= ■ 

Dp2p (Ccnt«r(PQ ,:, Mon_Ceri) , D 

mean(Mag) - ■ 
std«v(Mag) = ■ 



.Status'". 



0.01 



Count 500 
past 



RMS Distance Error V. Iteration Step 




1 1 


1 1 1 


» 1 1 


i m yu. 




1 1 1 — 


1 1 1 


i 1 



io; 

pass 



12 



14 



16 



18 



20 
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Conclusions: 

• " \ Eclge&matter,; need t6;be-:0:00i":&":^efto^j|e5j 0;00G1 has incredible results! . 

• Detail-on monument matersy the smro worse than the double peak; max (Count) = 132 
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6: All about the Skewed Data set, this Is XICP 



6A: Define the Initial 3D Transform Rotation and Translation values 



e Scan-^^ Ae Scan)r Ae Scan : 
I 



*X2S - ^Sc^P-Scan^Scan) 



Defined at bottom: These are the:IM motion of the scannerrelative to the X axis 

Pscan^M 2;A6 Scan)- Ae Scan Y S c^=° 



-3.067 |deg 
I 0 J 



' 0.995 -0;O87 ; 0.053\ 
RX2 S s 0:087 0.996; }0.005 | 
S -0.Q53 0 0.999/ 

M;987\ 

Fm 4Ariglcs(Rx2s) 



Now weidefine th e SCAN to- X frame 
Iran sformation. Thls.wiil be used to "skew* 
tnedata^ 

FRAME: it^wfllbe quantified in theXlCP 
stage. 



-3^67 \dtg. 



Scan,; 

R X2S := RI^PRN(conci(Mon_Namc , "Rx2s.txt")) Eriable'this to read in a matrix, disable to use the one defined above 



6B: Apply the 3D Transforms; to pertrub of "Skew'' the Scan frame away from the Xstage 



Maskx := 



'0 t 0 
0 1 0;| 
0 0 1 ) 

(\ 0 0^ 
0 0 0/| 
0 0 O) 



This mask lets us work only on the YZ values, e ? g. 

this mask lets us work only on the X yalues. e.g. 
Its also easy to undo... 



Here we learn to be Very Careful.. . In this version, we want to, appry the SKEW to the data (S2X) without any;X2M 
perturbation ^ and. with out any planar perturbations. We are given a hybrid point form the scanner 



If.we have no planer to c onsider,- its simpler to consider P H 



and: 









fx-] 


ft>1 ( 










M + 






U) • 




W) { 






if 



0 I = Rslx'M 3 ^' 



'l B C\ 
0 E F -Ph= Px 
,0 H 1/ 



y s | +.Masky 2 - 

r A 8 C} 

Rs2x = P E F 
G H I ) 



I = (R s2x Masli x + Masky^- 

then 



?X.= 

y, I 

N 



Y 

J) 



<A B C\ 


r 0 0 


'i o 0 s ! 




B <S\ 


D E F 


0 1 0 | > 


0 0 0 | = 


0 


E F 


,G H 1/ 


s° 0 


t 6- o o^ 


^0 


H Ij 
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We will apply them whole sale to all the points; NOTE: We.might be perturbing the P1 set or. th e original^ P ;0 . We want to 
test the two perturbations separately at first. 





'A B C\ 


'0 0 0% 




'p* Qy s + R-Zj^ 




'l o o\ 




'P + Q-y s + R z s ^ 


Y = 


D E .F . • 


0 1 0 |- 




b, i 


+, 


0 0 0: '{■. 


Vs. 1- 


0. T 


w ■ 


<0 H 1/ 


,o o i). 




I o .) L 











Y 



'A B C~) 

D :.E. F • 

• G h; I . ; 



^0 o o\ 
0 1 o I 



Ml 



1*1=0 



;Q:=X«oef^ 1> 

|q|=o 



(P+Qy s + Rz s Y 
0 t 

R:=Xcoef/ 4>; 



<1 0 0 i>| 
:6 0 O'J 

VP' 0 P'^ 



fP+Q-Ys + Rz^ 
0 I 
6 j 



<A b 4% 

D E F >:= RjQs 
<0 H ijj 



SkewYou(Input) :=.: 



Skcw(Input,R) := 



for re'O cols(Input) - 1 
M miiror 



;r l; 



Fi :(a-Qm + b) (a-Rm + g) 

0 : (b-QM+E) (D,R M + F) 

:Q (O Qm+h) (g rm + ^ 



dpm.I 



P«t 

M f;<F R-Maskj +Mask yz ," 
for i.€,0 . coW (Input) - 1. 

Out® <^Mlnput^ 
put. 



Unskcw(Input , R) := 



M ^ R Maskx + M%ky Z " 
for ■; i e 0 cols(input) - .1 

Ou$< 
Out 



Save, the way it was. 
It worked but was 
backwards 



UnskcwYou (Input , R) 



for i G 0 cols(Input) ; - 1 

Ax '4~ XcocfT % ; h + Xcocff 



n f Atycn ..-Input:. . .+'Xcoeff - 
mirror . 0 mirror, 1 ^ i,i ; mirror ,4 





Maskjj-InpuP + 




/ 


Oi#<-R- 


o I 


+ Mask^Input^ — 








V 



Out 



Skew(uiput .R) := 



M f- .R'Masl^ + Mask^ 

for 1 G 0 .. cols(Input) - 1 

Oirt^ <- M~ 1 Input^ 
Out 



Unskcw(Input.R):: 



M f-.R-Maskjj + Masky Z 
for i g 0.. cols (Input) - 1 
Out^ <r- Mlnput^ 



This works om 
NON-PL-VNER data 



Oiit 
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p skcw := ;SkcwYou(P5) p Define the "skewed" data set 
= 0.03117 



Compare the mean perturbation: 



6G: We need functions to Collate both the Dda and the Control sets 



Gonqrt-cssconjj-oi (Data; Control) : 



for i € 0 .. :cois(Data) - 1 



Ax c <-,Xco«fr . . ^4-Xcoeff ^Data, + Xcocff . Dai 

■ *. mirrof;0 ' mirror .1 ~ l.i : mirror;4 



Ou#>- Control <i ^- MMk^.Data^ > > 



0 J 



Out 



Gonqw-cssj) a ^ a (Data) := 



for i € 0 .v cols (Data) 1 

Ax_ <- Xcocff A .+ Xcoeff . ,-Data, i +fXcocff : ^Data^ .•« 

1 .mirror;0.= mirror,.! l,i ;»nurror.;4 P2.il 



Out <l> .<- Mask^-Data^* 



0 I 

i. 9 ) 



Out 



Con^rcss Gontro i (Data, Control) : 



for ie 0.. cols (Data).- 1 

Out <!> <r- Control - Mask^ Data^ 
Out 



Compress Data (Data) - = 



for i G 0 .. . cols(Data) - ;1 
Out® <~ !^k-.pata^ 



Out 



P skcwA :=s Comprcss Control (P skcw ,P 0 ) 
P skcwA := Compress Contro , (P 0 , P s kcw) 

P 0A := Comprcss Control (P 0 ,P skcw ) 



P 0A := GompressTj^P^) 
a( P 0) 



THIS WORKS when 
Skew/Unskew are done right! 

This seems like the way it should 
:be.;.., worked with old 
SkewAJnskew 



p skcwA := ,Com P rcss Data ( p o) The way it was before atl.the chari ges 

^P2P ( p skc wA » ?0 a) .= 0 03 1 1 7 Compare the mean perturbation, is it the same as yefore compression? 
p 0A' : = ^ S P( P 0A) p skcWA' - Di sp( p skcwA) 
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M 0A ,:= Find Gehter (P 0A ) Then we find the centers ( 

M skewA : = Find Center( p skewA) M 0A =" 

T 0A - Center(P 0A ,Mp A ) Then we apply the centers 
TskewA- Ge ^ er (PstewA^skewA) 

R AT .:= : Find R2 (T ske w Ar T 0A ) Now we find the rotation matrix with centering 
R A := Find R 2(P skewAf P 0A ) And without centering 

We look at many d iagnostics 



-8.707x10' I 
-0-015 ) 



f 3 \ 



M 



skewA = 



1.22x10 

-8.684xl0" 3 I 
-0.015 ) 



(■ 0.9973 0.02063 



R a > — 



0.07041 



-0.02069 0.99979 -2 .98234 x 10 
-0.07039 -0.00146 .0.99752: 



11 I Rat 

J 



' 0.9973 0:02063 0.07041 \ 

-0.02069 0:99979 -2,9/159 x 10* U I 
,-0-07039 -0-00146 0.99752 J 



r 0.9973 -0:02069 -0.07039 \ 
0.02063 0.99979. -0.00146 | 
^0.07041 0 0:99752 J 



Find 



Angles' 



( -1.18532 Y 
4.03747 |deg 



P 2 a : = Move 



p skewA» R A' 



oj. 



Find 



Angles 



( R AT ,T ) = 



' -1.18532 ^ 
4.03747 |deg 

, 1.69208 xl0" 9 J 

'-1.185321 



Find Angles( R X2s) = 4.03747 |deg 
0 J 

Apply the rotation and recenter the data set 
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P 2AT :=-Move(p skewA , Rat^ H&ew a^qa) Apply the rotation and: recenter the data set 

■%R(P^%Ati-= 5.50124x10" 11 

6D: We also need to Expand the Data back to its original extent 



P3 A r=-Un S kew;(p skew ,R A , T ) 
P 3AT ^ Unskew(p skew ,R AT ?*) 

P 3A .::= Disp(P 3A ): 



Here we call the function using the original and the recentered data 



P 3AT . := Disp(P 3AT ) 





Dp2p( P 0> P o);= 0 
D P2p( p skewvP.o) ^ 0 ^ 3117 



This is the RMS; after 'Skew but before XIGP. 
This: calculates against both P.O and. PA , 
depends on which data det was skewed 
above 



^P2p( p skewA' P 0A) = 0.03117 The RMS,did;n6t change it when; ; skewed 

Note: P OA got defined properly above 



%2p(?2A- p qA) .= 5-736x 10 XIGP fixed most of the RMS! 
d P2p( p 2AT> p 0a) = 5.735 x io" 9 XI CP fixed most of the RMS! 



_ .,_ jr Y _ 1A -9 Shouldn't have residual XI CP errors 

D P2P( p 3A> p 0j -'5 73571 x 10 once the data is expanded,.. 

rs « v c ucti irT 9 ^Shouldn't have residua 1X1 CP errors 

Dp2P( p 3AT> p oj = 5.73571 x.10 ; once the data is expanded... 

6F: We will need to move the projected Control Data to the Data frame 



Tahgo(pata,R,G d ^,C cont rQi) := 



for kG 0 .. cols(Data) - 1 

Dat^r <-,Data (k) - (C conlrol - R-C^) 



Data Offset 



Data Offiet <k> = RD ^ a< ' > + ( C contfol- R .Cdata). 



Dp2p( p i. p o) = 0 
Dp2p( p l > p o)=° 
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fixed this once" 



p 0Tango := Tango(P 0 ;R',M 1 .Mj,) 



»P2p(P0Tango.P 



o i: 
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XX:: The; Ballet A gorlthm does X1CP wrapped around X2M 



Ballet (Data) := 



SKEWjData «-Data 
TEMP_Data <- SKEW_Data; 

TEMPJData <- Center ( SKEWjData ^ Center (^KEW_Data) J if I^_CENTTER_FLAG 
™toJ$o> ^ G data ^control <- SUB_Control *r SUB^Data 4- 0: 
PirigPong <-r 0 

"k is the index for ft e outer Ballet loop" 
for k€ 0 .100 

"Only do outer XIGR if rough X2& is done" 
if ' PingPong -s 1 

X^SUB^Controi;^ Tango (sUB_Control , R^C^ ^control } 
COiMDP_X_SUB^I)ate <- Cott^ress Data (SOTlPata j 
GOMP_X_SUB_Gontrol 4- Coh^r«ss Go j l1ro i(SXJB i .Data,:X_STJBC Control). 
COMP.G^a ^ Rndcenter (CQMPJXjjira .Date J 
COMP_C^ onfro | ^ Find Ccntw .(COMP_X_:SUB_Control) 

GEN-Gcontrbl^ CentcrfcOMP.X^STJB.Control , COMP,C c6nlro i) 

Rout ^ ^^(^^^^Sc^ot 
Tc ^^ Knd Angics(Rout) 

Rout ^(t^);^^^!) 

SKEWjData <- UnskcwYou (Data , R out ) 
TEMP Data <- Movc(SKEW_Data, R^, C data> G couM ) 

'Inner X2M Loop" 
for pass €.0.. 20 

"Index for the trimmed array" 

"Go through ail the points in the data set" 
for me 0.. cols(TEMP_Data) - 1. 

"Find ihdeties for the closest triangles, could be array of several sister triangles" 
12 m ,<- Index2 (TEMP_Data^ m> ) 
"So, go through the array and test each one" 
for ie O ..rows(l2 m ) - 1 

TEMP^P ^ P nprm [ TEMPjW^ , (l2 m )J 
TEMP_S Tcs^ n 2pTEMP_P,(l2 m ).~| 

"Ah Ha, pick this one!" 
if TEMP_S = 1 

SUBJData^:<--Data (m > 
STjftiSKEW jW j > <- SKEW_Data< m > 
SUBJControi^ <- TEMP_P 
j .<-<!+' 1 
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^data ^^Genter (SUBiSKEWData) 
^control ^ K^Centcr( SlJB ^6ntrol) 

Ri„ +-'$B&g£fi^^ , , Center(SliB JControl , C conlrol )) 

TEMP^Data Move (SKEW J)ata , c data. c c6irtroi) 
TEOTjiJ^lData ^Mpye(SOTjK^:p^ 
if - PingPong = 0 



KMS-jj v <- Dp2p(SUB_Cbntrol , TEMPJSUBJData) 
■ .pass ~ ~ 

County <- j 
pass 



break' if :/RMS in -RMSj, 

\ pass-1 pass- 



[RMS: r 



<Tol) ; :(: m 

/ I pass-2 

- . a* «. 

break if PingPong.s 1 
VAfter.we get here the 1st time, we must pi ay Ping Pong until done" 
PingPohg;*- I if PingPong = 0 

"After every pass of XICP and then X2M, we get the chance to bail" 



r BMS^ <TolY if (pass > lo)> 
pass-1 ;/ 



break if 




-RMS; 



'TEMP_Data^ 


^TEMPiData^ 




RMSin 
County 




County 




'{3,136}^ 

im> 

{204} 




:= Ballet (Test) 


*in 










RMS but 






R out 

s ^ J 


■ 


R out 

, % ) 




<3,3> 



R.irris the rotation. matrix to take the data from X frame to MON frame 



0.99605 ,-0.08565 ^0.0233^ 
0.08729 0.99279: 0.08214 j 
^0 0161- -0.05385 0.99635 J 



R M2X 



f 099605 -0 08565 -0.0233 \ 
o\08729 0.99279 0.08214 | 
k 6.0161 -0.08385 0.99635/ 
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Knd Ang!cs(Rin) s 



'-5.0085^ 
0.92236 Jdcg 
,4:81052^ 



Fm 4Anglcs( R M2X^) = 



'-5;0O84?^ 
6.92235. |.dcg 
4.81052 ) 



|^Anglcs(Rin)| = 7 0054 675,dcg 



I |^Anglcs(Rin)h ^^^[r^^I )m ^5.333 X 10 



R.out is the rotation totakethe data from SCAN frame to X frame 



R out" 



' 0;9973 -0.02069 ^07039} 
0.02063 0199979 -0.00146 | 
,0:07041 0 0.99752 J 



Rod knglcs( R out) = 



-1.18529> 
4.0375, |dcg 



R X2S = 



,Find Angles( R X2s) 



'0:9973 ^0.0206? -0.07039 \ 

0.02063; .059979 -1.4565 X Kf 3 I 
,0.07041. 0 0:99752 ) 

'-4.18532^ 
4.03747 |dcg 

I o j 



| Fin <fAnglc S ( R oui)| = 420789 dc « 



MAnglcs( R X2s)| = 4-20787 dcg 

f I^Angles^out)] - |^Anglcs( R X2s)|) :2 ° = 5.533xl0~* 



The inner loop quits early (by deign) 
RMSj n ; ■■ -RMS: n , . = -438677 xl0~ 5 The differential improvement on the last pass of inner loop onK 

m rows(RMS in )-2 m rows(RMS in )-l 



-2 



VowsfRMS- ]~1 



= 1:44278 x 10 The magnitude on the last pass of inner loop only 



The outer loop takesover when the inner loop quits 



RMS 0Ut . - RMS out =7503x10 fhe differential improvement on the last pass of the oputer Ibq 

rows(RMS oUt J-2 rows^RMS^J- 1 



RMS 



°out 



= 3.978 x 10~ The magnitude oh the last pass of the. outer loop 



pass in := 0 .. rows(RMS| n ) - 1 
k:=0..rows(RMS out )- 1. 
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RMS Dist Error V. iteration Pass 




0™% 



— ■ ; 


1 


l "I " : ~T— 1 


, | . - r — .» F ..,— - 








cols(Tcst) 


1 




1 1 1 1 


i i i i 



v 0 
132 



8 



10: 



12 



14 



16: 



18 



20 



raax^Gpiintj^ 

5 Run XICP based on the setting in the "Control Panel" 



Control Panel 



Data Name = "FrontPeaks ;24 FULL SIM.txt" 



Points = 2000 

M6n_Name = "Fron"tPeaks_24_ 
Aedgc = 0:00001 

Far s;0.5 

Tol = 0.0000001 
Epsilon .a 0.000001 



mirror = 1 



PRE CENTER FLAG. 2 1 



The GAD file has a name... we would like to re-use it for the output files 

This sets how close a point can come to an edge an still be considered. XICP work better 
when. the points are allowed WAY CLOSE to the edge 

This sets how far away a point can be from an edge an still be considered, XICP work 
better when the: points are^ mildly Impressed 

by points: in the middle of a" face. 

This is an experimental variable used to let XICP shut itself off when its achieved "bottom 
out" behaviour. So far, looks like it must be set at 1 0 A -6 or lower or there is error still left to 
be,removed. try 0.000001 

Epsilon is used to control the "Safe Comparison which was added to -get around floating 
point errors when working with CAD data for the monument 

When Working with real or simulated data, we need to either keep track Of rriirVbr number 
( a big rewrite) or we can filter to only work with data from one mirror. This will control whic 
mirror is read in and make sure the correct line from the. Xceoff.ini file isread in. 

X2M/and XICP can handle larger initial off^ts of is used. However, 

when testingwith ppinb e MONUMENT, precentering just injects lerrors, so 

here iwe can turn it off ^ setting to "0". 
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Table:* 

Table 2 contains a Math 

to find the value of ^e. 



findcenter.mcd 

Basic 3D Vector and Homogeneous Coordinate Relationships 



(vo yi v 2 ^y 1 

H2V(v)i:=" — — ^ 
^V3 V3 V3J. 



T(x;y,z) := 



0 0 x\ 
0 1 :Q y I 
0 0 1 z [ 

s 0 rO -0 -t/ 



Ty(v) ^ f(vo>yi»;v2) 



Convert a homogenous point to a 3D vector. Useful for finding cross products 



Generated translation matrix given XYZ cobrdiates 



Generated translation matnx giveri a vector quantity 



R Axis( axi s\ angle) 



c <- cos (angle) ; 
sin(arigl'e) 

t<- i ^ c 

. .. axis 
axis.*— 7t" p 
| axis | 

x<— axisfj' 

y «- axisi 

z *-> axis2/ 

f 2 A 
t-x + c t-x;y - s z:. t-.x;z + s y : 0 ' 

trxy> s z fry 2 + c :fcyz-sx 0. 

t x z - s-y tyz + sx t-z? + c 0 
0 o q lj 



This can be done With quaternions... butthe 
quaternion math has been done and put 
directly in^matiixjfoiTn., (t isimattiematically 
the same as ^aternions; ;lt just simplifies to 
the. following matrix form whicftis how 
need the data to end up; 



R^^Caxis, angle) := mat <e~ R^g(axis , angle)' 

suDmafrix(mat , 0,2 ,6 ,2) 
R x ( angle) :== R^^l 0 0 )^, angle) These are the basic x.y.z axis rotations. 



Ry(an^e) R^^CO \ 0) T ; angle) 



; R ? (angle)M-R Axi g((0 0: 1 y\ang5le; 
Unit(vl) 

jV 

Vector Angle(vl;v2) :~ acos^- 



vl 
Ml 



vl v2- ^ 



Typically these are done as special cases 
but since we have the RotAxis function we 
can use it to provide a compact Way to write 
the specific orthogon a]' cases. 



Convert a vector to a unit vector 

This function calculates the angle between 
a pair of vectors in radians. 
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Find Center of Rotation Given Three 
3D Points In 3 Coordinate Frames 



1A: Run through the basic math for finding the center and axis of rotation 

The following runs through the math for finding the center of rotation. The algorithm finds the intersection 
of 3 planes. The result is a point and a vector defining an axis in 3 space. 



Points 



P, :-- 



1 1 

,0/ 



[0j 



fn 

o.l 



Bisector Points 



p bl 



p l +p 2 



P b2 * s 



P 2 +P 3 



m 

:vs\ 



P b2 = 



1 I 

w 



Segment Vectors 



v i : = (; p i,- p 2) ; 



v i = 



-i I 



V 2. : =(: p 3r p 2) : 



Axis of Rotation 



v r.-= ( v i*V 2 ) 









6-1 







Rotation Plane Offset 



BiSector Plan e Offsets 



Di = 0 



D 2 ■= -2 



Now we have 3 plan es, two for the bisectors and one for the plane of rotation, 
these 3 planes intersect at a point on the axis of rotation. 
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D := 



D =: 



-2 | 



N >.stoac^v 1 T ,Vj T > v r T ) 



N < 



fl -I 0 \ 

o ; -2 o | 



p center ^ N 



center " 



1 I 

by 



Find center of rotation By 
finding intersection of 3 
planes using a simple 
inverse 



N-P, 



center:' 



f:9l 
"2 I 
U J 



Verify by plugging center point 
back in to obtain Itte plane 
offsets 



center 



" p i| 



center 



-P 0 =1 



| P center^ P 3| ~ 1 



Distance from original three 
poi hts sh ould all be the same: 
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1B: Functi nf r finding center and axis f rotation 



RotationCenterfel.pZ,,*)^ 



•^ind^ectors usee! to find axjs^^tation" 
V j <- pi - pi 

•Y£<pp3.T- pi 

"Find Bisector points" 

***** 

'TFind^xis of rotation^ 
"FindPIane Offsets" 

%^v r P bl 

D r- V r p l 

"Generate a vector of plane offsets" 
"Generate matrix of normal vectors^ 



ve for the center vi a matrix inversion" 
center t-N^D 

"Return the center, unit axis and radius" 
r center Y 



Test:Gase> pi := 

center^ 

\ axis* jl 

center = 



rnd(2) I 
Lrn*(3)^ 



Srr 



p2 :=? 



rhd(i)^ 
md(2)| 



P3 * 



•rnd(l)\ 
md(2) I 
l:rnd(3)j 



:= RotationGenterCpl , p2 \ p3) 



'l,205;^ 

1.095, I axis ^ 



'0:873^ 
0:196/1 



Jpl - center) ■» 1.418 
|p2 center) .= 1:418 
|p3 - center) = 1418 
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Find Center of Rotation Given Four 
3D Points In 2 Coordinate Frames 



1C: Run through the basic math for finding the center and axis of rotation 

The basic algpritm is to fin d simi larto above ... Rnd two; planes using bisectors an d the third plane u sing -the normal 
vector from the cross product and assumingithat this plane has a zero D value '(this value may be arbitrarily 
chobsehX. With three jafe easily < be found by mattik^inversiori- 

{-2\ (2\ .m 



Given 4 points: 

Bisectors 

Vectors 

Orthogonal 



Bsi 



p ?2 '<r 



2 I. 



2 I Pbj ;= 



6| 



Pb 2 + P82 



'2 



V r :=V 2 XV i 



D i = v i *i Pi^Yr?^ D r 0 v i = 



center := stack^V 1 T > y 2 ^,V^ ; 



!P-2. 1 center 



PA 














V 2 = 


2 1 


Yr = 


Q 1 




U / 




U ; 




<24j 




f ° \ 








^6 -2 


91 


-2:| 


stack^Vj 


r t 

> V 2 - v r 




6 2 


o "I 


U ) 








,0 0 





[bsj - center] = 3.162 jpaj - ceriter| = 4.472 jpbj - centerj = 4.472 
[b^- center] = 3.162 |pa 2 - center] = 4.472 |l>t^ center] = 4.472 
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1 D: Write a Function that Finds the Center and Axis Given 2 Co rd in ate Frames. 



If is most likely that we will sdh/e this problem given 2 reference; frames so we'll write a function to solve this given 
two transformation matrices. From here we pick tw frame and resolve them i rtbleach 

transformation frame These 4 points\can be used to find bisector planes that are'intersected with cross product 
plane (arbitrarily fixed at x = 0). Thus the problem reduces to an intersection of 3 planes. 



RbtGOTterFromFrames^Tj , T^Jj 



pi:*- (o; 500 o thy 

p 4^ (0 0 500 I ) 1 



H2V 



(t 2 ;pi) 

Pb 2 <- H2y (T2-p2) 
Bsj <- 

BS£<- 



Pa^fPb^ 



center stock 



stack(center,l)\ 
M V r) j 



V 2 : .BS2 | 
; P ) 



Arbitary points 



Transformed into 2 Frames 



Find Bisectors 



Three planes 



Solve intersection of 
three planes 



Return arbitrary point on axis 
of rotation and me axis vector 



Calculate the distance from a point jq a lih e. This value will always be non negative 



Dist^Cpth^ec/vpth)- 



pt : ^ ( ptho pthi pth2 ) 

vpt t^ ( vptho - vpthi. vptbi ) 

u (pt-vptj-vec 

veovec 
(ptline*— vpt + t-vec) 
|pt- ptline| 
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1E: Aligning M nument Axis To Global Axis a 

With the fU notions for rotating about arbitrary axes an d fin cfi rig ' the an igl e ( between vectors we are ready to write 

frejequatibri tojribVe^frb^ 

firstfindth^cen^ 

coordinate -frame;. Finally, we trahdate the^ this location will 

likely be 0, Q, 0 but for completeness an arbitral rr^y be arbitrary : 





y r «- Unit( a^ x a^) 




angle r YectbiMgfle^,a^ 




™r^ R Axis(V a ^ e r} 












Rcerit€rW n, t; 



This function makes a matrix that takes 
you from the cl.al frame to the c2, a 2 
coordinate frame. 



axj := Unit 



'rnd(l)} 
rnd(l) |axj; = 



%'.146^ 
.0.982 I cn r *= 



2 
3 

0 



cn, + axi 



'1,146^ 
2;982 
3.118 . 



Pi 



1 
0 



Tg := AlignAxes^cnj , axj ,.cn 2 , ax 2 ) Tg = 



'0.146363 0.98213 0.118311 -0.465558^ 

-0.98213 6.158573 -0 101362 1.969069 

-0.118311 -0,101362 -2.642334 

0 0 0 1 ) 



Tg^ll 



y) 

(A 

1 

n| - 1 
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